

#include "Simulacion.h"


Simulacion::Simulacion()
{

}


Simulacion::Simulacion(double TimeStep, char *filenamePuntosOrg,
                       char *filenameIndicesOrg,
                       char **filenamePuntosInstrumento1,
                       char **filenameIndicesInstrumento1,
                       int NumeroMallas1,
                       char **filenamePuntosInstrumento2,
                       char **filenameIndicesInstrumento2,
                       int NumeroMallas2){
	dblTimeStep = TimeStep;
	charFnamePtosOrg = filenamePuntosOrg;
	charFnameIndsOrg = filenameIndicesOrg;
	
	charFnamePtosInst1 = filenamePuntosInstrumento1;
	charFnameIndsInst1 = filenameIndicesInstrumento1;
	intNroMallasInst1 = NumeroMallas1;

	charFnamePtosInst2 = filenamePuntosInstrumento2;
	charFnameIndsInst2 = filenameIndicesInstrumento2;
	intNroMallasInst2 = NumeroMallas2;
}

void Simulacion::IncializarSimulacion()
{

	if(enmTipoSesion == TRANSPORTAR_CUBOS)
	{
		blnHayColisionInst1 = false;
		blnHayColisionInst2 = false;

		for(int i=0;i<intNumeroCubos;i++)
		{
			ObjetoRigido objCubo;
			Triangulo t1;
			Triangulo t2;
			objCubo.set_TipoGeometria(SUP_TRIANGULAR);
			objCubo.set_FilenamePuntos(charFnamePtosCubo);
			objCubo.set_FilenameIndices(charFnameIndsCubo);
			objCubo.getSupTriang().set_TiposSesion(TRANSPORTAR_CUBOS);
			objCubo.CargarObjetoRigido();

			//Calculando planos de los triangulos
			for(int j=0;j<objCubo.getSupTriang().get_NroTriangs();j++){
				objCubo.getSupTriang().get_Triangulo(j).CalcularEcuacionPlano();
			}

			//Determinar conectividad triangulos
			objCubo.getSupTriang().DeterminarConectividad();


			//Cargando Cubo
			objCubos.push_back(objCubo);
			objTriangulosTemp1Cubo.push_back(t1);
			objTriangulosTemp2Cubo.push_back(t2);
		}

		objCubos[1].set_PosX(100.0);
		objCubos[1].set_PosY(0.0);
		objCubos[1].set_PosZ(0.0);

		objCubos[2].set_PosX(-100.0f);
		objCubos[2].set_PosY(0.0f);
		objCubos[2].set_PosZ(0.0f);

		//Inicializa el polhemus
		objPolhemusControl.InicializarPolhemus();
		//Inicializa la interfaz humano computador conectada al pto USB
		objUSBInstrumentoControl.AbrirPuerto();

		//Cargando Instrumento Qco1
		objIntrumentoQco1.set_TipoGeometria(SUP_TRIANGULAR);
		objIntrumentoQco1.set_FnamePtosInst(charFnamePtosInst1);
		objIntrumentoQco1.set_FnameIndsInst(charFnameIndsInst1);

		objIntrumentoQco1.set_NumeroMallas(intNroMallasInst1);
		objIntrumentoQco1.CargarInstrumento();

		//Cargando Instrumento Qco2
		objIntrumentoQco2.set_TipoGeometria(SUP_TRIANGULAR);
		objIntrumentoQco2.set_FnamePtosInst(charFnamePtosInst2);
		objIntrumentoQco2.set_FnameIndsInst(charFnameIndsInst2);

		objIntrumentoQco2.set_NumeroMallas(intNroMallasInst2);
		objIntrumentoQco2.CargarInstrumento();
	}
	else if(enmTipoSesion == ORDENAR_CUBOS)
	{
		blnHayColisionInst1 = false;
		blnHayColisionInst2 = false;

		for(int i=0;i<intNumeroCubos;i++)
		{
			ObjetoRigido objCubo;
			Triangulo t1;
			Triangulo t2;
			objCubo.set_TipoGeometria(SUP_TRIANGULAR);
			objCubo.set_FilenamePuntos(charFnamePtosCubo);
			objCubo.set_FilenameIndices(charFnameIndsCubo);
			objCubo.getSupTriang().set_TiposSesion(ORDENAR_CUBOS);
			objCubo.CargarObjetoRigido();

			//Calculando planos de los triangulos
			for(int j=0;j<objCubo.getSupTriang().get_NroTriangs();j++){
				objCubo.getSupTriang().get_Triangulo(j).CalcularEcuacionPlano();
			}

			//Determinar conectividad triangulos
			objCubo.getSupTriang().DeterminarConectividad();

			//Cargando Cubo
			objCubos.push_back(objCubo);
			objTriangulosTemp1Cubo.push_back(t1);
			objTriangulosTemp2Cubo.push_back(t2);
		}

		objCubos[1].set_PosX(100.0);
		objCubos[1].set_PosY(0.0);
		objCubos[1].set_PosZ(0.0);

		objCubos[2].set_PosX(-100.0f);
		objCubos[2].set_PosY(0.0f);
		objCubos[2].set_PosZ(0.0f);
	}
	else if(enmTipoSesion == SIMULACION_HIGADO)
	{
		blnHayColisionInst1 = false;
		blnHayColisionInst2 = false;

		//Cargando Organo
		objHigado.set_TipoGeometria(SUP_TRIANGULAR);
		objHigado.set_FilenamePuntos(charFnamePtosOrg);
		objHigado.set_FilenameIndices(charFnameIndsOrg);
		objHigado.getSupTriang().set_TiposSesion(SIMULACION_HIGADO);
		objHigado.CargarObjetoDeformable();
		objHigado.CargarJerarquia();
		objHigado.CrearModeloFisico();
	}
	else
	{
		throw new exception;
	}

    //Inicializando Timer
    objControlTimer.setTimerBreak(dblTimeStep);
}


void Simulacion::PararSimulacion()
{

}


void Simulacion::Simular()
{
	if(enmTipoSesion == TRANSPORTAR_CUBOS || enmTipoSesion == ORDENAR_CUBOS)
	{
		for(int i=0;i<intNumeroCubos;i++)
		{
			//Determinando si hay colision con el instrumento 1
			objCubos[i].CalcularEsfera(objTriangulosTemp1Cubo[i].getVertice1(),objTriangulosTemp2Cubo[i].getVertice1());
			objIntrumentoQco1.CalcularEsfera(objTrianguloTemp1);

			if(TestEsfera_Esfera(objIntrumentoQco1.get_Esfera(),objCubos[i].get_Esfera()))
			{
				blnHayColisionInst1 = true;
				intIDCuboInst1 = i;
				//cout << "HAY COLISION Instrumento 1  " << intIDCuboInst1 <<  endl;
			}

			//Determinando si hay colision con el instrumento 2
			objIntrumentoQco2.CalcularEsfera(objTrianguloTemp2);

			if(TestEsfera_Esfera(objIntrumentoQco2.get_Esfera(),objCubos[i].get_Esfera()))
			{
				blnHayColisionInst2 = true;
				intIDCuboInst2 = i;
				//cout << "HAY COLISION Instrumento 2  " << intIDCuboInst2 << endl;
			}
		}
	}
	else if(enmTipoSesion == SIMULACION_HIGADO)
	{
		//Determinando si hay colision con el instrumento 1
		if(objHigado.getArbolRitter().hayColision(objTrianguloTemp1))
		{
			intIDTriangColOrg1 = objHigado.getArbolRitter().getTrianguloCol().getID();
			//cout << "HAY COLISION " << "triangulo: " << intIDTriangColOrg1 <<  endl;

			blnHayColisionInst1 = true;

			int intIDVerticeColInst1 = objHigado.getSupTriang().get_Triangulo(intIDTriangColOrg1).getVertice1()->getID();

			if(objUSBInstrumentoControl.get_DatoPto1() < 210)
			{
				objHigado.getMass_Spring_Model().getOneNodo(intIDVerticeColInst1).getVertice()->setX(objTrianguloTemp1.getVertice1()->getX());
				objHigado.getMass_Spring_Model().getOneNodo(intIDVerticeColInst1).getVertice()->setY(objTrianguloTemp1.getVertice1()->getY());
				objHigado.getMass_Spring_Model().getOneNodo(intIDVerticeColInst1).getVertice()->setZ(objTrianguloTemp1.getVertice1()->getZ());
			}
		}
		else
		{
			//intIDTriangColOrg1 = objHigado.getArbolRitter().getTrianguloCol().getID();
			//cout << "No hay Colision" << endl;
			blnHayColisionInst1 = false;
		}

		//Determinando si hay colision con el instrumento 2
		if(objHigado.getArbolRitter().hayColision(objTrianguloTemp2))
		{
			intIDTriangColOrg2 = objHigado.getArbolRitter().getTrianguloCol().getID();
			//cout << "HAY COLISION " << "triangulo: " << intIDTriangColOrg2 <<  endl;
			blnHayColisionInst2 = true;

			int intIDVerticeColInst2 = objHigado.getSupTriang().get_Triangulo(intIDTriangColOrg2).getVertice1()->getID();

			if(objUSBInstrumentoControl.get_DatoPto2() < 210)
			{
				objHigado.getMass_Spring_Model().getOneNodo(intIDVerticeColInst2).getVertice()->setX(objTrianguloTemp2.getVertice1()->getX());
				objHigado.getMass_Spring_Model().getOneNodo(intIDVerticeColInst2).getVertice()->setY(objTrianguloTemp2.getVertice1()->getY());
				objHigado.getMass_Spring_Model().getOneNodo(intIDVerticeColInst2).getVertice()->setZ(objTrianguloTemp2.getVertice1()->getZ());
			}
		}
		else
		{
			//intIDTriangColOrg2 = objHigado.getArbolRitter().getTrianguloCol().getID();
			//cout << "No hay Colision" << endl;
			blnHayColisionInst2 = false;
		}
	    
		objControlTimer.InicializarTimer();
	    
		int intContador = 0;
	    
		while(!objControlTimer.CalcularTimer())
		{
			//cout << intContador << endl;
	        
			objHigado.getMass_Spring_Model().Calculate();
	        
			intContador++;
		}
	}
	else
	{
		throw new exception;
	}
}

void Simulacion::ActualizarInstrumentos()
{
#if defined (_WIN32)
    float x1, y1, z1, x2, y2, z2;
    float rx1, ry1, rz1, rx2, ry2, rz2;
	
    objPolhemusControl.ActualizarDatos();
	objUSBInstrumentoControl.LeerPuerto();
    
    x1 = objPolhemusControl.get_x1();
    y1 = objPolhemusControl.get_y1();
    z1 = objPolhemusControl.get_z1();
    
    x2 = objPolhemusControl.get_x2();
    y2 = objPolhemusControl.get_y2();
    z2 = objPolhemusControl.get_z2();
    
    rx1 = objPolhemusControl.get_rx1();
    ry1 = objPolhemusControl.get_ry1();
    rz1 = objPolhemusControl.get_rz1();

    rx2 = objPolhemusControl.get_rx2();
    ry2 = objPolhemusControl.get_ry2();
    rz2 = objPolhemusControl.get_rz2();
    
    objIntrumentoQco1.set_CoordenadaX(x1);
    objIntrumentoQco1.set_CoordenadaY(y1);
    objIntrumentoQco1.set_CoordenadaZ(z1);
    
    objIntrumentoQco2.set_CoordenadaX(x2);
    objIntrumentoQco2.set_CoordenadaY(y2);
    objIntrumentoQco2.set_CoordenadaZ(z2);
    
    objIntrumentoQco1.set_RotacionX(rx1);
    objIntrumentoQco1.set_RotacionY(ry1);
    objIntrumentoQco1.set_RotacionZ(rz1);
    
    objIntrumentoQco2.set_RotacionX(rx2);
    objIntrumentoQco2.set_RotacionY(ry2);
    objIntrumentoQco2.set_RotacionZ(rz2);

	objIntrumentoQco1.set_GradosPuntaInstrumento(objUSBInstrumentoControl.get_DatoPto1());
	objIntrumentoQco2.set_GradosPuntaInstrumento(objUSBInstrumentoControl.get_DatoPto2());

#endif
#if defined (__linux__)
    float x1, y1, z1;
    float rx1, ry1, rz1;
    
    objPolhemusControl.ActualizarDatos();
    
    x1 = objPolhemusControl.get_x1();
    y1 = objPolhemusControl.get_y1();
    z1 = objPolhemusControl.get_z1();
    
 
    rx1 = objPolhemusControl.get_rx1();
    ry1 = objPolhemusControl.get_ry1();
    rz1 = objPolhemusControl.get_rz1();
 
    objIntrumentoQco1.set_CoordenadaX(x1);
    objIntrumentoQco1.set_CoordenadaY(y1);
    objIntrumentoQco1.set_CoordenadaZ(z1);
  
    objIntrumentoQco1.set_RotacionX(rx1);
    objIntrumentoQco1.set_RotacionY(ry1);
    objIntrumentoQco1.set_RotacionZ(rz1);
#endif

    
}

